﻿
using System;
using System.Collections.Generic;
using TheDarkKnight.MeshFactory;
using UnityEngine;
using static TheDarkKnight.TempletObjectBase;

namespace TheDarkKnight.DynamicSpwanModel
{

    /// <summary>
    /// 动态模型生成算法库
    /// 基于beizer曲线路径，根据切面模板模型，动态采样生成一组模型
    /// 作者：徐雨同
    /// 日期: 2021年6月1日
    /// </summary>
    public class DynamicModelLibrary
    {

        public enum CurveModel
        {
            NORMAL,          //正常模式
            LASTONESIDE,     //承上单边模式
            NEXTONESIDE,     //启下单边模式
            BOTHSIDE         //双边模式
        }


        public enum TextureType
        {
            Tile,//普通平铺模式
            Texture//贴图模式
        }

        public enum SpwanModelType
        {
            New,
            Refresh
        }

        /// <summary>
        /// 限定容错角度
        /// </summary>
        private const float LIMITANGLE = 30;

        /// <summary>
        /// 动态生成模型
        /// ****************************
        ///      南无阿弥陀佛
        ///      苦海无涯，回头是岸
        /// ****************************
        /// </summary>
        public static void DynamicSpwanModel(TempletObjectBase TemplteMesh, List<Vector3> pathList, List<Vector3> beizerList, Action<MeshPlus, Material[], Vector2> finishCallback, TextureType textrueType = TextureType.Texture, int segment = 10)
        {

            MeshTemplet meshTemplet = ParseModelTemplet(TemplteMesh);
            int templteVerticesCount = meshTemplet.Vertices.Length;
            int templteTriangleCapcity = meshTemplet.Triangels.Length;
            int pathCount = pathList.Count - 1;
            float UTotalLength = 0;
            float bezierLength = 0;
            int vCapacity = templteVerticesCount * (pathCount * (segment - 1) + 3);
            int tCapacity = templteVerticesCount * (pathCount * (segment - 2) + pathCount) * 6;
            int sCapacity = 2;
            Material surfaceMat = TemplteMesh.SurfaceMaterial;
            Material sectionMat = TemplteMesh.SectionMaterial;
            Vector3[] templteVertice = meshTemplet.Vertices;

            MeshShell tunnelShell = new MeshShell(vCapacity, tCapacity, sCapacity);
            tunnelShell.InitMesh(Quaternion.identity);
            UTotalLength = BeizerUilityTools.GetTotalLength(templteVertice);
            int tempVerticeCount = 0;
            for (int pathIndex = 0; pathIndex < pathCount; pathIndex++)
            {
                Vector3 curNode = pathList[pathIndex];
                Vector3 nextNode = pathList[pathIndex + 1];
                Vector3 bezierNode = beizerList[pathIndex];
                float length = BeizerUilityTools.BezierLength(curNode, nextNode, bezierNode);
                bezierLength += length;
                for (int segmentIndex = pathIndex == 0 ? 0 : 1; segmentIndex < segment; segmentIndex++)
                {
                    float t = segmentIndex * 1.0f / (segment - 1);
                    Vector3 segmentPoint = BeizerUilityTools.BezierCurve(curNode, nextNode, bezierNode, t);
                    Vector3 gradient = BeizerUilityTools.BezierCurveGradient(curNode, nextNode, bezierNode, t);
                    float tempLength = 0;
                    for (int verticesIndex = 0; verticesIndex < templteVerticesCount; verticesIndex++)
                    {
                        Vector3 localPos = templteVertice[verticesIndex];
                        Vector3 worldPos = localToWorld(localPos, segmentPoint, gradient);
                        tunnelShell.AddVertices(tempVerticeCount, worldPos);
                        tunnelShell.AddNormals(tempVerticeCount, (templteVertice[verticesIndex] - segmentPoint).normalized);
                        if (verticesIndex > 0) tempLength += Vector3.Distance(templteVertice[verticesIndex], templteVertice[verticesIndex - 1]);
                        tunnelShell.AddUV(tempVerticeCount, new Vector2(tempLength / UTotalLength, 0));
                        tempVerticeCount++;
                    }
                }
            }
            //TODO 添加UV
            float[] totalDistanceArray = new float[templteVerticesCount];
            float[] eachVerticesDistanceArray = new float[tempVerticeCount];
            for (int templteIndex = 0; templteIndex < templteVerticesCount; templteIndex++)
            {
                float totalDistance = 0;
                eachVerticesDistanceArray[templteIndex] = 0;
                for (int uIndex = templteIndex; uIndex < tempVerticeCount; uIndex += templteVerticesCount)
                {
                    int nextIndex = uIndex + templteVerticesCount;
                    if (nextIndex > (tempVerticeCount - 1)) continue;
                    Vector3 curPoint = tunnelShell.Vertices[uIndex];
                    Vector3 nextPoint = tunnelShell.Vertices[nextIndex];
                    totalDistance += CaculateSqrDistance(curPoint, nextPoint);
                    eachVerticesDistanceArray[nextIndex] = totalDistance;
                }
                totalDistanceArray[templteIndex] = totalDistance;
            }
            for (int i = 0; i < tempVerticeCount; i++)
            {
                int index = i % templteVerticesCount;
                float t = eachVerticesDistanceArray[i] / totalDistanceArray[index];
                tunnelShell.UV[i] = new Vector2(tunnelShell.UV[i].x, t);
            }
            int triangleIndex = 0;
            for (int pathIndex = 0; pathIndex < (pathCount + (pathCount) * (segment - 2)); pathIndex++)
            {
                for (int pointIndex = 0; pointIndex < templteVerticesCount - 1; pointIndex++)
                {
                    int currentOne = templteVerticesCount * pathIndex + pointIndex;
                    int currentTwo = templteVerticesCount * pathIndex + (pointIndex + 1) % (templteVerticesCount);
                    int nextOne = templteVerticesCount * (pathIndex + 1) + pointIndex;
                    int nextTwo = templteVerticesCount * (pathIndex + 1) + (pointIndex + 1) % (templteVerticesCount);
                    tunnelShell.AddTriangles(triangleIndex, currentOne, currentTwo, nextOne);
                    triangleIndex += 3;
                    tunnelShell.AddTriangles(triangleIndex, nextOne, currentTwo, nextTwo);
                    triangleIndex += 3;
                }
            }

            List<int> forwardSubMeshTriangels = GetSubTriangelsList(tempVerticeCount, meshTemplet, templteTriangleCapcity);
            Vector3 firstGradient = -BeizerUilityTools.BezierCurveGradient(pathList[0], pathList[1], beizerList[0], 0);
            AddMaskTemplet(meshTemplet, tunnelShell, pathList[0], firstGradient, ref tempVerticeCount);
            tunnelShell.AddSubTriangles(0, forwardSubMeshTriangels);

            List<int> backSubMeshTriangels = GetSubTriangelsList(tempVerticeCount, meshTemplet, templteTriangleCapcity);
            Vector3 backGradient = BeizerUilityTools.BezierCurveGradient(pathList[pathList.Count - 2], pathList[pathList.Count - 1], beizerList[pathList.Count - 2], 1);
            AddMaskTemplet(meshTemplet, tunnelShell, pathList[pathList.Count - 1], backGradient, ref tempVerticeCount);
            tunnelShell.AddSubTriangles(1, backSubMeshTriangels);

            MeshPlus mesh = tunnelShell.GetMesh(meshTemplet, segment);
            Material[] materials = new Material[] {
                    surfaceMat,
                    sectionMat,
                    sectionMat
            };
            Vector2 textureTile = Vector2.one;
            if (textrueType == TextureType.Tile)
                textureTile = new Vector2(UTotalLength, bezierLength);
            else
                textureTile = new Vector2(1, bezierLength / UTotalLength);
            if (finishCallback != null)
            {
                finishCallback(mesh, materials, textureTile);
            }
        }

        /// <summary>
        /// 动态刷新网格数据
        /// </summary>
        /// <param name="TemplteMesh"></param>
        /// <param name="pathList"></param>
        /// <param name="beizerList"></param>
        /// <param name="refreshMesh"></param>
        /// <param name="finishCallback"></param>
        public static void DynamicSpwanRefreshModel(MeshPlus refreshMesh, List<Vector3> pathList, List<Vector3> beizerList, Action callback)
        {
            int templteVerticesCount = refreshMesh.Templete_Vertices.Length;
            int pathCount = pathList.Count - 1;
            float bezierLength = 0;
            int meshVerticesCount = refreshMesh.MeshData.vertexCount;
            Vector3[] templeteVertice = refreshMesh.Templete_Vertices;
            Vector3[] refreshVertice = refreshMesh.Vertices;
            Vector3[] refreshNormal = refreshMesh.Normals;
            Vector2[] refreshUV = refreshMesh.UV;
            int verticeIndex = 0;
            for (int pathIndex = 0; pathIndex < pathCount; pathIndex++)
            {
                Vector3 curNode = pathList[pathIndex];
                Vector3 nextNode = pathList[pathIndex + 1];
                Vector3 bezierNode = beizerList[pathIndex];
                float length = BeizerUilityTools.BezierLength(curNode, nextNode, bezierNode);
                bezierLength += length;
                for (int segmentIndex = pathIndex == 0 ? 0 : 1; segmentIndex < refreshMesh.Segment; segmentIndex++)
                {
                    float t = segmentIndex * 1.0f / (refreshMesh.Segment - 1);
                    Vector3 segmentPoint = BeizerUilityTools.BezierCurve(curNode, nextNode, bezierNode, t);
                    Vector3 gradient = BeizerUilityTools.BezierCurveGradient(curNode, nextNode, bezierNode, t);
                    for (int verticesIndex = 0; verticesIndex < templteVerticesCount; verticesIndex++)
                    {
                        Vector3 localPos = templeteVertice[verticesIndex];
                        Vector3 worldPos = localToWorld(localPos, segmentPoint, gradient);
                        refreshVertice[verticeIndex] = worldPos - Vector3.zero;
                        refreshNormal[verticeIndex] = (templeteVertice[verticesIndex] - segmentPoint).normalized;
                        verticeIndex++;
                    }
                }
            }
            //TODO 添加UV
            int curVertice = meshVerticesCount - 2 * templteVerticesCount;
            float[] totalDistanceArray = new float[templteVerticesCount];
            for (int templteIndex = 0; templteIndex < templteVerticesCount; templteIndex++)
            {
                float totalDistance = 0;
                for (int uIndex = templteIndex; uIndex < verticeIndex; uIndex += templteVerticesCount)
                {
                    int nextIndex = uIndex + templteVerticesCount;
                    if (nextIndex > verticeIndex) break;
                    Vector3 curPoint = refreshVertice[uIndex];
                    Vector3 nextPoint = refreshVertice[nextIndex];
                    totalDistance += CaculateSqrDistance(curPoint, nextPoint);
                }
                totalDistanceArray[templteIndex] = totalDistance;
            }
            for (int i = 0; i < curVertice; i++)
            {
                Vector3 curPoint = refreshVertice[i];
                int index = i % templteVerticesCount;
                Vector3 BackPoint = curPoint;
                float currentDistance = 0;
                for (int b = i - templteVerticesCount; b >= 0; b -= templteVerticesCount)
                {
                    Vector3 tempPoint = refreshVertice[b];
                    currentDistance += CaculateSqrDistance(tempPoint, BackPoint);
                    BackPoint = tempPoint;
                }
                float t = currentDistance / totalDistanceArray[index];
                Vector2 uv = refreshUV[i];
                uv.y = t;
                refreshUV[i] = uv;
            }
            Vector3 firstGradient = -BeizerUilityTools.BezierCurveGradient(pathList[0], pathList[1], beizerList[0], 0);
            RefreshMaskVertice(pathList, templteVerticesCount, templeteVertice, refreshVertice, verticeIndex, firstGradient, pathList[0]);
            verticeIndex += templteVerticesCount;
            Vector3 backGradient = BeizerUilityTools.BezierCurveGradient(pathList[pathList.Count - 2], pathList[pathList.Count - 1], beizerList[pathList.Count - 2], 1);
            RefreshMaskVertice(pathList, templteVerticesCount, templeteVertice, refreshVertice, verticeIndex, backGradient, pathList[pathList.Count - 1]);
            refreshMesh.MeshData.SetVertices(refreshVertice);
            refreshMesh.MeshData.SetNormals(refreshNormal);
            refreshMesh.MeshData.uv = refreshUV;
            refreshMesh.MeshData.RecalculateNormals();
            refreshMesh.MeshData.RecalculateTangents();
            if (callback != null)
                callback();
        }

        private static float CaculateSqrDistance(Vector3 start, Vector3 end)
        {
            float valueX = (start.x - end.x);
            float valutY = (start.y - end.y);
            float valutZ = (start.z - end.z);
            return valueX * valueX + valutY * valutY + valutZ * valutZ;
        }


        private static void RefreshMaskVertice(List<Vector3> pathList, int verticesCount, Vector3[] templeteVertice, Vector3[] refreshVertice, int verticeIndex, Vector3 firstGradient, Vector3 parentPos)
        {
            for (int index = 0; index < verticesCount; index++)
            {
                Vector3 localPos = templeteVertice[index];
                Vector3 worldPos = localToWorld(localPos, parentPos, firstGradient);
                refreshVertice[verticeIndex + index] = worldPos;
            }
        }


        /// <summary>
        /// 获取路径曲线的采样点
        /// </summary>
        /// <param name="pathList"></param>
        /// <param name="beizerList"></param>
        /// <param name="simpleCount"></param>
        /// <returns></returns>
        public static List<Vector3> TakeSimplesBezierPointList(List<Vector3> pathList, List<Vector3> beizerList, int simpleCount = 10)
        {
            List<Vector3> SimpleList = new List<Vector3>();
            for (int pathIndex = 0; pathIndex < pathList.Count - 1; pathIndex++)
            {
                Vector3 c = pathList[pathIndex];
                Vector3 n = pathList[pathIndex + 1];
                Vector3 b = beizerList[pathIndex];
                for (int sI = pathIndex == 0 ? 0 : 1; sI < simpleCount; sI++)
                {
                    float t = sI * 1.0f / (simpleCount - 1);
                    Vector3 sP = BeizerUilityTools.BezierCurve(c, n, b, t);
                    SimpleList.Add(sP);
                }
            }
            return SimpleList;
        }

        private static void AddMaskTemplet(MeshTemplet meshTemplet, MeshShell tunnelShell, Vector3 curPosition, Vector3 firstGradient, ref int verticeIndex)
        {
            for (int verticesIndex = 0; verticesIndex < meshTemplet.Vertices.Length; verticesIndex++)
            {
                Vector3 localPos = meshTemplet.Vertices[verticesIndex];
                Vector3 worldPos = localToWorld(localPos, curPosition, firstGradient);
                //添加定点坐标
                tunnelShell.AddVertices(verticeIndex, worldPos);
                //添加法线
                tunnelShell.AddNormals(verticeIndex, meshTemplet.Normals[verticesIndex]);
                //添加UV
                tunnelShell.AddUV(verticeIndex, meshTemplet.UV[verticesIndex]);
                verticeIndex++;
            }
        }


        private static List<int> GetSubTriangelsList(int verticeCount, MeshTemplet meshTemplet, int tCapacity)
        {
            List<int> subMeshTriangels = new List<int>(tCapacity);
            for (int triangelIndex = 0; triangelIndex < meshTemplet.Triangels.Length; triangelIndex += 3)
            {
                subMeshTriangels.Add(verticeCount + meshTemplet.Triangels[triangelIndex]);
                subMeshTriangels.Add(verticeCount + meshTemplet.Triangels[triangelIndex + 1]);
                subMeshTriangels.Add(verticeCount + meshTemplet.Triangels[triangelIndex + 2]);
            }

            return subMeshTriangels;
        }

        private static MeshTemplet ParseModelTemplet(TempletObjectBase modelTemplet)
        {
            int vecticesCount = modelTemplet.Vectices.Count;
            int triangleCount = modelTemplet.Triangles.Count;

            Vector3[] vertices = new Vector3[vecticesCount];
            Vector2[] UV = new Vector2[vecticesCount];
            Vector3[] normal = new Vector3[vecticesCount];
            int[] triangles = new int[triangleCount * 3];
            for (int i = 0; i < vecticesCount; i++)
            {
                VerticesData data = modelTemplet.Vectices[i];
                vertices[i] = data.Vertices;
                UV[i] = data.UV;
                normal[i] = data.Normal;
            }
            int triangleIndex = 0;
            for (int j = 0; j < triangleCount; j++)
            {
                TriangelData data = modelTemplet.Triangles[j];
                triangles[triangleIndex] = data.FirstVertice;
                triangles[triangleIndex + 1] = data.SecondVertice;
                triangles[triangleIndex + 2] = data.ThridVertice;
                triangleIndex += 3;
            }
            return new MeshTemplet(vertices, triangles, normal, UV);
        }

        /// <summary>
        /// 自动获取起始与结束当前路径点的斜率
        /// </summary>
        /// <param name="pathList"></param>
        /// <param name="beizerList"></param>
        /// <param name="startGradient"></param>
        /// <param name="endGradient"></param>
        public static void CatchStartAndEndGradient(List<Vector3> pathList, List<Vector3> beizerList, out Vector3 startGradient, out Vector3 endGradient)
        {
            startGradient = BeizerUilityTools.BezierCurveGradient(pathList[0], pathList[1], beizerList[0], 0).normalized;
            endGradient = BeizerUilityTools.BezierCurveGradient(pathList[pathList.Count - 2], pathList[pathList.Count - 1], beizerList[pathList.Count - 2], 1).normalized;
        }

        /// <summary>
        /// 自动计算贝塞尔曲线
        /// </summary>
        public static List<Vector3> AutoCaculateBezierCurve(List<Vector3> pathList, bool isNeedCure = true, CurveModel model = CurveModel.NORMAL, float checkAnagle = 15, float fixedAngle = 10, Vector3 lastGradient = default(Vector3), Vector3 nextGradient = default(Vector3))
        {
            List<Vector3> beizerList = new List<Vector3>(pathList.Count - 1);
            for (int beizerIndex = 0; beizerIndex < pathList.Count - 1; beizerIndex++)
            {
                beizerList.Add((pathList[beizerIndex + 1] + pathList[beizerIndex]) / 2);
            }
            if (isNeedCure && pathList.Count > 1)
            {
                switch (model)
                {
                    case CurveModel.NORMAL:
                        AutoCurveByNormal(pathList, beizerList, checkAnagle, fixedAngle);
                        break;
                    case CurveModel.LASTONESIDE:
                        beizerList[0] = BeizerUilityTools.GetBezierCurePiontByGradient(lastGradient, pathList[0], pathList[1], 0);
                        AutoCurveByOneSide(pathList, beizerList, checkAnagle, fixedAngle);
                        break;
                    case CurveModel.NEXTONESIDE:
                        beizerList[beizerList.Count - 1] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, pathList[pathList.Count - 1], pathList[pathList.Count - 2], 0);
                        AutoCurveByOneSide(pathList, beizerList, checkAnagle, fixedAngle, false);
                        break;
                    case CurveModel.BOTHSIDE:
                        if (pathList.Count > 2)
                        {
                            beizerList[0] = BeizerUilityTools.GetBezierCurePiontByGradient(lastGradient, pathList[0], pathList[1], 0);
                            beizerList[beizerList.Count - 1] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, pathList[pathList.Count - 2], pathList[pathList.Count - 1], 1);
                            AutoCurveByBothSide(pathList, beizerList, checkAnagle, fixedAngle);
                        }
                        else
                        {
                            Vector3 middlePoint = (pathList[0] + pathList[1]) / 2;
                            beizerList[0] = CaculateBeizerNodeByTwoLine(VectorConversionToSlope(lastGradient), VectorConversionToSlope(nextGradient), pathList[0], pathList[1], middlePoint);
                        }
                        break;
                }
            }
            return beizerList;
        }

        private static void AutoCurveByBothSide(List<Vector3> pathList, List<Vector3> beizerList, float checkAnagle, float fixedAngle)
        {
            for (int i = 0; i < pathList.Count - 3; i++)
            {
                Vector3 preStartNode = new Vector3(pathList[i].x, 0, pathList[i].z);
                Vector3 preEndNode = new Vector3(pathList[i + 1].x, 0, pathList[i + 1].z);
                Vector3 preBeizerNode = new Vector3(beizerList[i].x, 0, beizerList[i].z);

                Vector3 nextStartNode = new Vector3(pathList[i + 1].x, 0, pathList[i + 1].z);
                Vector3 nextEndNode = new Vector3(pathList[i + 2].x, 0, pathList[i + 2].z); ;
                Vector3 nextBeizerNode = new Vector3(beizerList[i + 1].x, 0, beizerList[i + 1].z);

                Vector3 preGradient = BeizerUilityTools.BezierCurveGradient(preStartNode, preEndNode, preBeizerNode, 1);
                Vector3 nextGradient = BeizerUilityTools.BezierCurveGradient(nextStartNode, nextEndNode, nextBeizerNode, 0);

                Vector3 normal = Vector3.Cross(preGradient, nextGradient).normalized;

                float angle = Vector3.Angle(preGradient, nextGradient);
                if (angle > checkAnagle)
                {
                    float lerp = angle - fixedAngle;
                    nextGradient = Quaternion.AngleAxis(-lerp, normal) * nextGradient;
                    beizerList[i + 1] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, nextStartNode, nextEndNode, 0);
                    beizerList[i + 1] = new Vector3(beizerList[i + 1].x, (pathList[i + 1].y + pathList[i + 2].y) / 2, beizerList[i + 1].z);
                }
            }
            int pathCount = pathList.Count - 1;
            Vector3 eStartNode = new Vector3(pathList[pathCount - 1].x, 0, pathList[pathCount - 1].z);
            Vector3 eEndNode = new Vector3(pathList[pathCount].x, 0, pathList[pathCount].z);
            Vector3 eBeizerNode = new Vector3(beizerList[pathCount - 1].x, 0, beizerList[pathCount - 1].z);

            Vector3 lastStartNode = new Vector3(pathList[pathCount - 2].x, 0, pathList[pathCount - 2].z);
            Vector3 lastEndNode = new Vector3(pathList[pathCount - 1].x, 0, pathList[pathCount - 1].z); ;
            Vector3 lastBeizerNode = new Vector3(beizerList[pathCount - 2].x, 0, beizerList[pathCount - 2].z);

            Vector3 pGradient = BeizerUilityTools.BezierCurveGradient(eStartNode, eEndNode, eBeizerNode, 0);
            Vector3 nGradient = BeizerUilityTools.BezierCurveGradient(lastStartNode, lastEndNode, lastBeizerNode, 1);

            float lerpAngle = Vector3.Angle(pGradient, nGradient);
            if (lerpAngle > LIMITANGLE)
            {
                beizerList[beizerList.Count - 1] = (pathList[pathCount - 1] + pathList[pathCount]) / 2;
            }
        }

        private static void AutoCurveByOneSide(List<Vector3> pathList, List<Vector3> beizerList, float checkAnagle, float fixedAngle, bool IsBegainWithStart = true)
        {
            if (IsBegainWithStart)
            {
                for (int i = 0; i < pathList.Count - 2; i++)
                {
                    Vector3 preStartNode = new Vector3(pathList[i].x, 0, pathList[i].z);
                    Vector3 preEndNode = new Vector3(pathList[i + 1].x, 0, pathList[i + 1].z);
                    Vector3 preBeizerNode = new Vector3(beizerList[i].x, 0, beizerList[i].z);

                    Vector3 nextStartNode = new Vector3(pathList[i + 1].x, 0, pathList[i + 1].z);
                    Vector3 nextEndNode = new Vector3(pathList[i + 2].x, 0, pathList[i + 2].z); ;
                    Vector3 nextBeizerNode = new Vector3(beizerList[i + 1].x, 0, beizerList[i + 1].z);

                    Vector3 preGradient = BeizerUilityTools.BezierCurveGradient(preStartNode, preEndNode, preBeizerNode, 1);
                    Vector3 nextGradient = BeizerUilityTools.BezierCurveGradient(nextStartNode, nextEndNode, nextBeizerNode, 0);

                    Vector3 normal = Vector3.Cross(preGradient, nextGradient).normalized;

                    float angle = Vector3.Angle(preGradient, nextGradient);
                    if (angle > checkAnagle)
                    {
                        float lerp = angle - fixedAngle;
                        nextGradient = Quaternion.AngleAxis(-lerp, normal) * nextGradient;
                        beizerList[i + 1] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, nextStartNode, nextEndNode, 0);
                        beizerList[i + 1] = new Vector3(beizerList[i + 1].x, (pathList[i + 1].y + pathList[i + 2].y) / 2, beizerList[i + 1].z);
                    }
                }
            }
            else
            {
                for (int i = pathList.Count - 1; i > 1; i--)
                {

                    Vector3 curStartNode = new Vector3(pathList[i].x, 0, pathList[i].z);
                    Vector3 curEndNode = new Vector3(pathList[i - 1].x, 0, pathList[i - 1].z);
                    Vector3 curBeizerNode = new Vector3(beizerList[i - 1].x, 0, beizerList[i - 1].z);


                    Vector3 nextStartNode = new Vector3(pathList[i - 1].x, 0, pathList[i - 1].z);
                    Vector3 nextEndNode = new Vector3(pathList[i - 2].x, 0, pathList[i - 2].z);
                    Vector3 nextBeizerNode = new Vector3(beizerList[i - 2].x, 0, beizerList[i - 2].z);

                    Vector3 curGradient = BeizerUilityTools.BezierCurveGradient(curStartNode, curEndNode, curBeizerNode, 1);
                    Vector3 nextGradient = BeizerUilityTools.BezierCurveGradient(nextStartNode, nextEndNode, nextBeizerNode, 0);


                    Vector3 normal = Vector3.Cross(curGradient, nextGradient).normalized;

                    float angle = Vector3.Angle(curGradient, nextGradient);
                    if (angle > checkAnagle)
                    {
                        float lerp = angle - fixedAngle;
                        //float scale = preLength / (nextLength + preLength);
                        nextGradient = Quaternion.AngleAxis(-lerp, normal) * nextGradient;
                        beizerList[i - 2] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, nextStartNode, nextEndNode, 0);
                        beizerList[i - 2] = new Vector3(beizerList[i - 2].x, (pathList[i - 1].y + pathList[i - 2].y) / 2, beizerList[i - 2].z);
                    }
                }
            }
        }


        /// <summary>
        /// 平滑路径
        /// </summary>
        /// <param name="pathList">路径点</param>
        /// <param name="beizerList">贝塞尔曲线</param>
        /// <param name="checkAnagle">检测曲线角度阈值</param>
        /// <param name="fixedAngle">修复曲线角度阈值</param>
        private static void AutoCurveByNormal(List<Vector3> pathList, List<Vector3> beizerList, float checkAnagle, float fixedAngle)
        {
            for (int i = 0; i < pathList.Count - 2; i++)
            {
                Vector3 preStartNode = new Vector3(pathList[i].x, 0, pathList[i].z);
                Vector3 preEndNode = new Vector3(pathList[i + 1].x, 0, pathList[i + 1].z);
                Vector3 preBeizerNode = new Vector3(beizerList[i].x, 0, beizerList[i].z);
                float preLength = (preEndNode - preStartNode).magnitude;

                Vector3 nextStartNode = new Vector3(pathList[i + 1].x, 0, pathList[i + 1].z);
                Vector3 nextEndNode = new Vector3(pathList[i + 2].x, 0, pathList[i + 2].z); ;
                Vector3 nextBeizerNode = new Vector3(beizerList[i + 1].x, 0, beizerList[i + 1].z);
                float nextLength = (nextEndNode - nextStartNode).magnitude;

                Vector3 preGradient = BeizerUilityTools.BezierCurveGradient(preStartNode, preEndNode, preBeizerNode, 1);
                Vector3 nextGradient = BeizerUilityTools.BezierCurveGradient(nextStartNode, nextEndNode, nextBeizerNode, 0);

                Vector3 normal = Vector3.Cross(preGradient, nextGradient).normalized;

                float angle = Vector3.Angle(preGradient, nextGradient);
                if (angle > checkAnagle)
                {
                    float lerp = angle - fixedAngle;
                    float scale = preLength / (nextLength + preLength);
                    if (i == 0)
                    {
                        preGradient = Quaternion.AngleAxis(lerp * scale, normal) * preGradient;
                        beizerList[i] = BeizerUilityTools.GetBezierCurePiontByGradient(preGradient, preStartNode, preEndNode, 1);
                        beizerList[i] = new Vector3(beizerList[i].x, (pathList[i].y + pathList[i + 1].y) / 2, beizerList[i].z);
                        nextGradient = Quaternion.AngleAxis(-lerp * (1 - scale), normal) * nextGradient;
                        beizerList[i + 1] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, nextStartNode, nextEndNode, 0);
                        beizerList[i + 1] = new Vector3(beizerList[i + 1].x, (pathList[i + 1].y + pathList[i + 2].y) / 2, beizerList[i + 1].z);
                    }
                    else
                    {
                        nextGradient = Quaternion.AngleAxis(-lerp, normal) * nextGradient;
                        beizerList[i + 1] = BeizerUilityTools.GetBezierCurePiontByGradient(nextGradient, nextStartNode, nextEndNode, 0);
                        beizerList[i + 1] = new Vector3(beizerList[i + 1].x, (pathList[i + 1].y + pathList[i + 2].y) / 2, beizerList[i + 1].z);
                    }
                }
            }
        }

        private static float VectorConversionToSlope(Vector3 dir)
        {
            return dir.z / dir.x;
        }

        private static Vector3 CaculateBeizerNodeByTwoLine(float startGradient, float endGradient, Vector3 startPoint, Vector3 endPoint, Vector3 middlePoint)
        {
            if (startGradient == endGradient)
            {
                Debug.LogError("斜率相等，无法计算贝赛点");
                return middlePoint;
            }
            float bezierX = (startGradient * startPoint.x - endGradient * endPoint.x - startPoint.z + endPoint.z) / (startGradient - endGradient);
            float beizerY = startGradient * (bezierX - startPoint.x) + startPoint.z;
            return new Vector3(bezierX, middlePoint.y, beizerY);
        }

        private static Vector3 localToWorld(Vector3 localPos, Vector3 parentPos, Vector3 forward)
        {
            if (forward == Vector3.zero)
            {
                return parentPos;
            }
            else
            {
                return parentPos + Quaternion.LookRotation(forward) * localPos;
            }

        }
    }
}
